#include <pit/pit.h>
#include <board.h>
#include <cp15/cp15.h>

#define BLOCK_SIZE 512
#define N_BLOCKS 450

#define RESOLUTION 0xfffff
#define MASK 0xfffff

#define N_MEASUREMENTS 1000

#define VALUEOF(piir) (((piir >> 20) * RESOLUTION) + (piir & MASK))

#define OFFSET_TABLE      0x8000

#if defined (AT91C_IRAM_1)
#define SRAM_ADDRESS  AT91C_IRAM_1
#define SRAM_SIZE     AT91C_IRAM_1_SIZE
#elif defined(AT91C_IRAM)
#define SRAM_ADDRESS  AT91C_IRAM
#define SRAM_SIZE     AT91C_IRAM_SIZE
#elif defined(AT91C_ISRAM)
#define SRAM_ADDRESS  AT91C_ISRAM
#define SRAM_SIZE     AT91C_ISRAM_SIZE
#else
#error SRAM define
#endif

#define SDRAM_ADDRESS 0x20000000

unsigned int *pTranslationTable = (unsigned int *) (SRAM_ADDRESS + OFFSET_TABLE);

//------------------------------------------------------------------------------
/// MMU Initialization
/// \param pTranslationTable  Translation table adresse
//------------------------------------------------------------------------------
static void InitMMU()
{
    int i;
    int addSDRAM;

    // Program the TTB
    _writeTTB((unsigned int) pTranslationTable);

    // Program the domain access register
    _writeDomain(0xC0000000); // domain 15: access are not checked

    // Reset table entries
    for (i = 0; i < 4096; i++) {
        pTranslationTable[i] = 0;
    }

    // Program level 1 page table entry
    // Vector adress
    pTranslationTable[0x0] = (0x00000000) | // Physical Address
            (1 << 10) | // Access in supervisor mode
            (15 << 5) | // Domain
            (1 << 4) | (0 << 3) | // No D cache
            0x2; // Set as 1 Mbyte section

    pTranslationTable[SRAM_ADDRESS >> 20] = (SRAM_ADDRESS) | // Physical Address
                    (1 << 10) | // Access in supervisor mode
                    (15 << 5) | // Domain
                    (1 << 4) | (1 << 3) | // D cache
                    0x2; // Set as 1 Mbyte section

    // SDRAM adress (with D cache)
    for (addSDRAM = (SDRAM_ADDRESS >> 20); addSDRAM < (SDRAM_ADDRESS >> 20) + 64; ++addSDRAM)
        pTranslationTable[addSDRAM] = (addSDRAM << 20) | // Physical Address
                (1 << 10) | // Access in supervisor mode
                (15 << 5) | // Domain
                (1 << 4) | (1 << 3) | // D cache
                0x2; // Set as 1 Mbyte section

    // Peripherals adress
    pTranslationTable[0xFFF] = (0xFFF00000) | // Physical Address
            (1 << 10) | // Access in supervisor mode
            (15 << 5) | // Domain
            (1 << 4) | (0 << 3) | // No D cache
            0x2; // Set as 1 Mbyte section
}

int main(void)
{
    unsigned int start;
    unsigned int end;
    unsigned long long start_time;
    unsigned long long end_time;

    unsigned char lcd[N_BLOCKS * BLOCK_SIZE];
    unsigned char img[N_BLOCKS * BLOCK_SIZE];

    unsigned int i, x, y;

    InitMMU();
    CP15_EnableMMU();
    CP15_Enable_D_Cache();

    /* Initialize PIT */
    PIT_Init(RESOLUTION, 1e9);
    PIT_Enable();

    /* Get the start PIVR value */
    start = PIT_GetPIIR();

    /* Lengthy operation */
    for (i = 0; i < N_MEASUREMENTS; ++i)
        for (y = 0; y < 320; ++y)
            for (x = 0; x < 240; ++x) {
                lcd[3 * (x + y)] = img[3 * ((320 - 1 - y) + x)];
                lcd[3 * (x + y) + 1] = img[3 * ((320 - 1 - y) + x) + 1];
                lcd[3 * (x + y) + 2] = img[3 * ((320 - 1 - y) + x) + 2];
            }

    /* Get the end PIVR value */
    end = PIT_GetPIIR();

    start_time = VALUEOF(start);
    end_time = VALUEOF(end);

    end_time -= start_time;
    end_time /= N_MEASUREMENTS;

    return 0;
}
